# --coding:utf-8--

# Copyright 2021 xpstem.com
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.

import unittest
from lelink.ev3brickclient import *
from lelink.ev3device import *
from lelink.conn.wificonnection import *

class EV3SensorTest(unittest.TestCase):
    def setUp(self) -> None:
        self._conn = WifiConnection()
        self._conn.connect()

        self._brick_client = EV3BrickClient(Layer.L0)
        self._brick_client.connect(self._conn)

    def tearDown(self) -> None:
        self._conn.disconnect()

    def _test_link(self, in_port: InputPort, except_type: int):
        device: EV3InputDevice = self._brick_client.get_inputdevice(in_port)

        self.assertIsNotNone(device, "Port {} empty.".format(in_port))
        self.assertEqual(except_type, device.type_, "Wrong device type.")

        print("mode: {}".format(device.mode))

    def _test_read(self, in_port: InputPort):
        device: EV3InputDevice = self._brick_client.get_inputdevice(in_port)

        raw = device.ready_raw(-1)
        pct = device.ready_pct(-1)
        si = device.ready_si(-1)
        print("raw:{}, pct:{}, si:{}".format(raw, pct, si))


class EV3TouchSensorTest(EV3SensorTest):

    # Touch sensor on port 1
    def test_link(self):
        self._test_link(InputPort.P1, EV3TouchSensor.TYPE)

    def test_read(self):
        self._test_read(InputPort.P1)


class EV3UltrasonicSensorTest(EV3SensorTest):

    # Ultrasonic sensor on port 2
    def test_link(self):
        self._test_link(InputPort.P2, EV3UltrasonicSensor.TYPE)

    def test_read(self):
        self._test_read(InputPort.P2)


class EV3GyroSensorTest(EV3SensorTest):

    # Gyro sensor on port 3
    def test_link(self):
        self._test_link(InputPort.P3, EV3GyroSensor.TYPE)

    def test_read(self):
        self._test_read(InputPort.P3)


class EV3ColorSensorTest(EV3SensorTest):

    # Color sensor on port 4
    def test_link(self):
        self._test_link(InputPort.P4, EV3ColorSensor.TYPE)

    def test_read(self):
        self._test_read(InputPort.P4)


class EV3IRSensorTest(EV3SensorTest):

    # IR sensor on port 4
    def test_link(self):
        self._test_link(InputPort.P4, EV3IRSensor.TYPE)

    def test_read(self):
        self._test_read(InputPort.P4)


class EV3ServoMotorTest(unittest.TestCase):

    def setUp(self) -> None:
        self._conn = WifiConnection()
        self._conn.connect()

        self._brick_client = EV3BrickClient(Layer.L0)
        self._brick_client.connect(self._conn)

    def tearDown(self) -> None:
        self._conn.disconnect()

    # Large motor on port A
    def test_link(self):
        device: EV3OutputDevice = self._brick_client.get_outputdevice(OutputPort.A)

        self.assertIsNotNone(device, "Port A empty.")
        self.assertEqual(EV3LargeMotor.TYPE, device.type_, "Not servo motor.")

        print("mode: {}".format(device.mode))

    def test_read(self):
        device: EV3InputDevice = self._brick_client.get_inputdevice(InputPort.A)

        self.assertIsNotNone(device, "Port A empty.")

        raw = device.ready_raw(EV3ServoMotor.MODE_DEGREE)
        pct = device.ready_pct(EV3ServoMotor.MODE_DEGREE)
        si = device.ready_si(EV3ServoMotor.MODE_DEGREE)
        print("raw:{}, pct:{}, si:{}".format(raw, pct, si))

    def test_steppower(self):
        device: EV3ServoMotor = self._brick_client.get_outputdevice(OutputPort.A)

        self.assertIsNotNone(device, "Port A empty.")

        device.start_on_steppower(50, 180, 720, 180, Brake.BREAK)


if __name__ == '__main__':
    unittest.main()
